/*
  Copyright (c) 2010 Les Newell. All rights reserved

  This program is free software: you can redistribute it and/or modify
  it under the terms of the GNU General Public License as published by
  the Free Software Foundation, either version 3 of the License, or
  (at your option) any later version.

  This program is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  GNU General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with this program. If not, see <http://www.gnu.org/licenses/>.
*/

//define the module.
DEFINE_MODULE(FlightMode,Module)
//Note the lack of an opening curly bracket.

    //Only use this to expose sockets and parameters. Do most of your
    //initialisastion in Init()
    FlightMode()
    {
        /*Expose sockets to the outside world
        NOTE: the socket name must be a static string.
        Bad Things will happen if you use a string allocated on the stack
        You can use a pointer to a static string.*/
        AddSocket("FlightMode.Status", &m_mode);
        m_mode = FLIGHTMODE_DISARMED;
    }

    //A parameter has changed. This happens at bootup if the EEPROM
    //contains valid data and at run-time if the user changes a
    //parameter over the serial link
    virtual void ParamChanged(Socket * param)
    {
    }

    //Initialise the module
    //set up hardware etc
    virtual void Init()
    {
        //Set how often Loop() should be executed (in milliseconds)
        SetInterval(50);
        SetPriority(30);
        m_receiver[PITCH].Connect("Receiver.Pitch");
        m_receiver[ROLL].Connect("Receiver.Roll");
        m_receiver[YAW].Connect("Receiver.Yaw");
        m_receiver[THROTTLE].Connect("Receiver.Throttle");
        m_accels[AXISX].Connect("Accel.X");
        m_accels[AXISY].Connect("Accel.Y");
        m_accels[AXISZ].Connect("Accel.Z");
        m_switch1.Connect("Receiver.Switch.2");
        m_acceleration.Connect("AHRS.Acceleration");
    }

    //Here is where you do your work. It is called at the rate defined by SetInterval
    virtual void Loop(const unsigned long& interval)
    {
        switch (m_mode)
        {
        case FLIGHTMODE_DISARMED:
            if (m_receiver[THROTTLE] < 0.1 && // no throttle
                    m_receiver[YAW] > 0.9 && // right hand rudder
                    m_receiver[PITCH] > 0.9 && // Full back elevator
                    m_receiver[ROLL] > 0.9) // right hand aileron
            {
                m_mode = FLIGHTMODE_ARMED;
            }
            break;

        case FLIGHTMODE_ARMED:
            if (m_receiver[THROTTLE] > 0.1)
            {
                m_oldSwitch = 100;
                g_moduleManager.Reference();
                m_mode = FLIGHTMODE_ACRO;
                m_oldSwitch = m_switch1;
            }
            else if (m_receiver[YAW] < -0.9 && //left hand rudder
                     m_receiver[PITCH] > 0.9 && // Full back elevator
                     m_receiver[ROLL] < -0.9) // left hand aileron
            {
                m_mode = FLIGHTMODE_DISARMED;
            }
            break;

        case FLIGHTMODE_ACRO:
        case FLIGHTMODE_HHOLD:
            if (m_receiver[THROTTLE] < 0.1 && //zero throttle
                    (m_accels[AXISZ] < -0.9 || m_accels[AXISZ] > 0.9) && //roughly level or upside down
                    fabs(m_accels[AXISX]) < 0.3 &&
                    fabs(m_accels[AXISY]) < 0.3)
            {
                m_mode = FLIGHTMODE_ARMED;
//                g_moduleManager.Reference();
                return;
            }
            if (m_switch1 != m_oldSwitch)
            {
                m_oldSwitch = m_switch1;
                if(m_oldSwitch > 0)
                {
                    m_mode = FLIGHTMODE_ACRO;
                }
                else
                {
                    m_mode = FLIGHTMODE_HHOLD;
                }
            }
            if(m_receiver[THROTTLE] < 0.1 || m_acceleration > 2.5)
            {
                m_mode = FLIGHTMODE_ACRO;
            }
        }
    }

private:
    PlugF m_accels[MAX_AXES];
    PlugF m_receiver[MAX_COMMANDS];
    PlugF m_yawCmd;
    PlugF m_acceleration;
    Plug8 m_switch1;
    byte m_oldSwitch;

    Socket8 m_mode;
};

FlightMode g_FlightMode;

